figure;
title('trj');
dxyz_cal = pos2dxyz(ins_cal(:,7:9));
dxyz_real = pos2dxyz(ins_real(:,7:9));
dxyz_yan = pos2dxyz(ins_yan(:,7:9));
plot(dxyz_real(:,1),dxyz_real(:,2),'r');xygo('est','nth');
hold on;
plot(dxyz_cal(:,1),dxyz_cal(:,2),'b');
hold on;
plot(dxyz_yan(:,1),dxyz_yan(:,2),'g');
% plot(dxyz_yan(:,1),dxyz_yan(:,2),'g');
%A3V2P2*2 

t = 1:1:len;
t2 = 1:nn:len;
%绘制误差曲线

figure;%真值与估计值之间误差
title('calculate vs true value');
subplot(3,1,1);plot(t2,ins_cal(:,1:3));subtitle('attitude');legend('rho','pitch','yaw');%attitude error
subplot(3,1,2);plot(t2,ins_cal(:,4:6));subtitle('velocity');legend('x','y','z');%velocity error
subplot(3,1,3);plot(t2,dxyz_cal(:,1:3));subtitle('position');legend('x','y','z');%position error



figure(2);%真值与估计值之间误差
title('calculate vs true value');
subplot(3,1,1);plot(t2,ins_real(1:nn:end,1:3)-ins_cal(:,1:3));subtitle('attitude');legend('rho','pitch','yaw');%attitude error
subplot(3,1,2);plot(t2,ins_real(1:nn:end,4:6)-ins_cal(:,4:6));subtitle('velocity');legend('x','y','z');%velocity error
subplot(3,1,3);plot(t2,dxyz_real(1:nn:end,1:3)-dxyz_cal(:,1:3));subtitle('position');legend('x','y','z');%position error
figure(3);%真值与严老师之间
title('yan vs true value');
subplot(3,1,1);plot(t2,ins_real(1:nn:end,1:3)-ins_yan(:,1:3));subtitle('attitude');legend('rho','pitch','yaw');%attitude error
subplot(3,1,2);plot(t2,ins_real(1:nn:end,4:6)-ins_yan(:,4:6));subtitle('velocity');legend('x','y','z');%velocity error
subplot(3,1,3);plot(t2,dxyz_real(1:nn:end,1:3)-dxyz_yan(:,1:3));subtitle('position');legend('x','y','z');%position error
figure(4);%严老师与估计之间
title('calculate vs yan');
subplot(3,1,1);plot(t2,ins_yan(:,1:3)-ins_cal(:,1:3));subtitle('attitude');legend('rho','pitch','yaw');%attitude error
subplot(3,1,2);plot(t2,ins_yan(:,4:6)-ins_cal(:,4:6));subtitle('velocity');legend('x','y','z');%velocity error
subplot(3,1,3);plot(t2,dxyz_yan(:,1:3)-dxyz_cal(:,1:3));subtitle('position');legend('x','y','z');%position error